/*------------------------------------------------------------------------------
* rtksvr.c : rtk server functions
*
*          Copyright (C) 2007-2020 by T.TAKASU, All rights reserved.
*
* options : -DWIN32    use WIN32 API
*
* version : $Revision:$ $Date:$
* history : 2009/01/07  1.0  new
*           2009/06/02  1.1  support glonass
*           2010/07/25  1.2  support correction input/log stream
*                            supoort online change of output/log streams
*                            supoort monitor stream
*                            added api:
*                                rtksvropenstr(),rtksvrclosestr()
*                            changed api:
*                                rtksvrstart()
*           2010/08/25  1.3  fix problem of ephemeris time inversion (2.4.0_p6)
*           2010/09/08  1.4  fix problem of ephemeris and ssr squence upset
*                            (2.4.0_p8)
*           2011/01/10  1.5  change api: rtksvrstart(),rtksvrostat()
*           2011/06/21  1.6  fix ephemeris handover problem
*           2012/05/14  1.7  fix bugs
*           2013/03/28  1.8  fix problem on lack of glonass freq number in raw
*                            fix problem on ephemeris with inverted toe
*                            add api rtksvrfree()
*           2014/06/28  1.9  fix probram on ephemeris update of beidou
*           2015/04/29  1.10 fix probram on ssr orbit/clock inconsistency
*           2015/07/31  1.11 add phase bias (fcb) correction
*           2015/12/05  1.12 support opt->pppopt=-DIS_FCB
*           2016/07/01  1.13 support averaging single pos as base position
*           2016/07/31  1.14 fix bug on ion/utc parameters input
*           2016/08/20  1.15 support api change of sendnmea()
*           2016/09/18  1.16 fix server-crash with server-cycle > 1000
*           2016/09/20  1.17 change api rtksvrstart()
*           2016/10/01  1.18 change api rtksvrstart()
*           2016/10/04  1.19 fix problem to send nmea of single solution
*           2016/10/09  1.20 add reset-and-single-sol mode for nmea-request
*           2017/04/11  1.21 add rtkfree() in rtksvrfree()
*           2020/11/30  1.22 add initializing svr->nav in rtksvrinit()
*                            allocate double size ephemeris in rtksvrinit()
*                            handle multiple ephemeris sets in updatesvr()
*                            use API sat2freq() to get carrier frequency
*                            use integer types in stdint.h
*-----------------------------------------------------------------------------*/
#include "rtklib.h"
#include "../app/comm.h"

#define MIN_INT_RESET   30000   /* mininum interval of reset command (ms) */

static kalman_t yawklm={0};

/* update antenna position ---------------------------------------------------*/
static void update_antpos(rtksvr_t *svr, int index)
{
    sta_t *sta;
    double pos[3],del[3]={0},dr[3];
    int i;

	if (svr->rtk.opt.refpos==POSOPT_RTCM&&index==1) {
		sta=&svr->rtcm[1].sta;
		/* update base station position */
		for (i=0;i<3;i++) {
			svr->rtk.rb[i]=sta->pos[i];
		}
		/* antenna delta */
		ecef2pos(svr->rtk.rb,pos);
		if (sta->deltype) { /* xyz */
			del[2]=sta->hgt;
			enu2ecef(pos,del,dr);
			for (i=0;i<3;i++) {
				svr->rtk.rb[i]+=sta->del[i]+dr[i];
			}
		}
		else { /* enu */
			enu2ecef(pos,sta->del,dr);
			for (i=0;i<3;i++) {
				svr->rtk.rb[i]+=dr[i];
			}
		}
	}
}
#if ENPPP
/* update ssr corrections ----------------------------------------------------*/
static void update_ssr(rtksvr_t *svr, int index)
{
    int i,sys,prn,iode;

	for (i=0;i<MAXSAT;i++) {
		if (!svr->rtcm[index].ssr[i].update) continue;
		
		/* check consistency between iods of orbit and clock */
		if (svr->rtcm[index].ssr[i].iod[0]!=svr->rtcm[index].ssr[i].iod[1]) {
			continue;
		}
		svr->rtcm[index].ssr[i].update=0;
		
		iode=svr->rtcm[index].ssr[i].iode;
		sys=satsys(i+1,&prn);
		
		/* check corresponding ephemeris exists */
		if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS) {
			if (svr->nav.eph[i       ].iode!=iode&&
				svr->nav.eph[i+MAXSAT].iode!=iode) {
				continue;
			}
		}
		else if (sys==SYS_GLO) {
			if (svr->nav.geph[prn-1          ].iode!=iode&&
				svr->nav.geph[prn-1+MAXPRNGLO].iode!=iode) {
				continue;
			}
		}
		svr->nav.ssr[i]=svr->rtcm[index].ssr[i];
	}
}
#endif
/* update rtk server struct --------------------------------------------------*/
static void update_svr(rtksvr_t *svr, int ret, int index)
{
    tracet(4,"updatesvr: ret=%d index=%d\n",ret,index);

	if (ret==5) { /* antenna postion */
		update_antpos(svr,index);
	}		
#if ENPPP
    if (ret==10) { /* ssr message */
        update_ssr(svr,index);
    }
#endif
}
#if DQUE_BUFFER_QTY==0
/* decode receiver raw/rtcm data ---------------------------------------------*/
static int decoderaw(rtksvr_t *svr, int index, int c)
{
    int ret,fobs=0;
    
    tracet(5,"decoderaw: index=%d\n",index);
    
    rtksvrlock(svr);
    
	ret=input_rtcm3(&svr->rtcm[index],c);
	if (ret) {
		/* decode rtcm3 message */
		ret = decode_rtcm3(&svr->rtcm[index], &svr->nav);
		/* update rtk server */
		if (ret>0) {
			update_svr(svr,ret,&svr->rtcm[index].obs,index);
			if (ret==1) {
				/* observation data received */
				fobs=1;
			}
		}
	}
    
    rtksvrunlock(svr);
    
    return fobs;
}
#else
/* decode receiver raw/rtcm data ---------------------------------------------*/
static int decoderaw(rtksvr_t *svr, int index)
{
    int c,ret,fobs=0;
    
    tracet(5,"decoderaw: index=%d\n",index);
    
    rtksvrlock(svr);
    
    while(1) {
		c = uart_GetChar(svr->stream[index]);
		if (c<0) break;

		if (input_rtcm3(&svr->rtcm[index],c)) {
			/* decode rtcm3 message */
			ret = decode_rtcm3(&svr->rtcm[index], &svr->nav);
			/* update rtk server */
			if (ret>0) {
				update_svr(svr,ret,index);
				if (ret==1) {
					sortobs(&svr->rtcm[index].obs);
					/* observation data received */
					fobs=1;
					break;
				}
			}
		}
    }
    
    rtksvrunlock(svr);
    
    return fobs;
}
#endif

#if ENPPP
/* carrier-phase bias (fcb) correction ---------------------------------------*/
static void corr_phase_bias(obsd_t *obs, int n, const nav_t *nav)
{
    double freq;
    uint8_t code;
    int i,j;
    
    for (i=0;i<n;i++) for (j=0;j<NFREQ;j++) {
        code=obs[i].code[j];
        if ((freq=sat2freq(obs[i].sat,code,nav))==0.0) continue;
        
        /* correct phase bias (cyc) */
        obs[i].L[j]-=nav->ssr[obs[i].sat-1].pbias[code-1]*freq/CLIGHT;
    }
}
#endif

/* output solution as the form of e/n/u-baseline -----------------------------*/
static void outyaw(const rtk_t *rtk,int cputime)
{
	double len,dir,k,pos[3],bl[3],enu[3],qr[6],lenb=rtk->opt.baseline[0];
	char stime[32],str[256];
	int i,nbl=0;

	if (rtk->sol.stat!=SOLQ_FIX) return;
#if 0
	for (i=0;i<8;i++) nbl+=rtk->nblist[i];
	nbl/=8;
	i=nbl/3;
	if ((nbl-rtk->nb_ar)>i) return;
#endif
	for (i=0;i<3;i++) bl[i]=rtk->sol.rr[i]-rtk->rb[i];
	len=norm(bl,3);
	if (fabs(len-lenb)>(lenb*0.03)) return;
	ecef2pos(rtk->rb,pos);
	ecef2enu(pos,bl,enu);
	dir=atan2(enu[0],enu[1])*R2D;
	if (dir<0) dir+=360.0;
	k=kalman(&yawklm, dir, 10.0, 0.02, 3.0);
	pos[0]*=R2D;
	pos[1]*=R2D;
	rndata.lat=pos[0]*1E6+0.5;
	rndata.lon=pos[1]*1E6+0.5;
	rndata.alt=pos[2]*10.0+0.5;
	rndata.bear=k*10.0+0.5;
	rndata.tilt=dir*10.0+0.5;
	time2str(rtk->sol.time,stime,0);
	for (i=0;i<6;i++) qr[i]=rtk->sol.qr[i];
	sprintf(str,"$YAW,%s,%.2f,%.2f,%.3f,%02d,%f,%.10f,%.10f,%f,%d\n",
			stime,dir,k,len,rtk->nb_ar,norm(qr,6)*1000,pos[0],pos[1],pos[2],cputime);
#ifdef TRACE
	trace(0,str);
#else
	aisg_debug(str);
#endif
}

/* rtk server thread ---------------------------------------------------------*/
static void rtksvrthread(void *arg)
{
    rtksvr_t *svr=(rtksvr_t *)arg;
    double tt;
    uint32_t tick;
    int i,j;
    
    tracet(3,"rtksvrthread:\n");
    
    for (; ;) {
        tick=tickget();
#if DQUE_BUFFER_QTY
        for (i=0;i<NSTAQ;i++) {
			/* decode receiver raw/rtcm data */
			if (decoderaw(svr,i)) SETBIT(svr->obsf, i);
        }
#endif
		if ((svr->obsf&3)==3) { /* for each rover observation data */
			svr->obsf=0;
#if ENPPP
            /* carrier phase bias correction */
            corr_phase_bias(svr->rtcm[0].obs.data,svr->rtcm[0].obs.n,&svr->nav);
			corr_phase_bias(svr->rtcm[1].obs.data,svr->rtcm[1].obs.n,&svr->nav);
#endif
            /* rtk positioning */
            rtksvrlock(svr);
            rtkpos(&svr->rtk,&svr->rtcm[0].obs,&svr->rtcm[1].obs,&svr->nav);
            rtksvrunlock(svr);
            
			svr->cputime=(int)(tickget()-tick);
            if (svr->rtk.sol.stat!=SOLQ_NONE) {
                /* adjust current time */
                tt=(int)(tickget()-tick)/1000.0+DTTOL;
                timeset(gpst2utc(timeadd(svr->rtk.sol.time,tt)));
				outyaw(&svr->rtk,svr->cputime);
            }
        }
		/* sleep until next cycle */
		sleepms(1);
    }
}
/* lock/unlock rtk server ------------------------------------------------------
* lock/unlock rtk server
* args   : rtksvr_t *svr    IO rtk server
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
extern void rtksvrlock  (rtksvr_t *svr) {lock  (&svr->lock);}
extern void rtksvrunlock(rtksvr_t *svr) {unlock(&svr->lock);}

#if DQUE_BUFFER_QTY==0
extern void rtksvrrecv(void *arg)
{
    rtksvr_t *svr=(rtksvr_t *)arg;
	u_word2 uv;
	int i;

	for (;;) {
		uv.n=dque_Wait();
		i=(uv.word[1]&0x7)-1;
		if (decoderaw(svr,i,uv.word[0])) SETBIT(svr->obsf, i);
	}
}
#endif
/* start rtk server ------------------------------------------------------------
* start rtk server thread
* args   : rtksvr_t *svr    IO rtk server
*          int     cycle    I  server cycle (ms)
*          int     buffsize I  input buffer size (bytes)
*          int     *strs    I  stream types (STR_???)
*                              types[0]=input stream rover
*                              types[1]=input stream base station
*                              types[2]=input stream correction
*                              types[3]=output stream solution 1
*                              types[4]=output stream solution 2
*                              types[5]=log stream rover
*                              types[6]=log stream base station
*                              types[7]=log stream correction
*          char    *paths   I  input stream paths
*          int     *format  I  input stream formats (STRFMT_???)
*                              format[0]=input stream rover
*                              format[1]=input stream base station
*                              format[2]=input stream correction
*          int     navsel   I  navigation message select
*                              (0:all,1:rover,2:base,3:corr)
*          char    **cmds   I  input stream start commands
*                              cmds[0]=input stream rover (NULL: no command)
*                              cmds[1]=input stream base (NULL: no command)
*                              cmds[2]=input stream corr (NULL: no command)
*          char    **cmds_periodic I input stream periodic commands
*                              cmds[0]=input stream rover (NULL: no command)
*                              cmds[1]=input stream base (NULL: no command)
*                              cmds[2]=input stream corr (NULL: no command)
*          char    **rcvopts I receiver options
*                              rcvopt[0]=receiver option rover
*                              rcvopt[1]=receiver option base
*                              rcvopt[2]=receiver option corr
*          int     nmeacycle I nmea request cycle (ms) (0:no request)
*          int     nmeareq  I  nmea request type
*                              (0:no,1:base pos,2:single sol,3:reset and single)
*          double *nmeapos  I  transmitted nmea position (ecef) (m)
*          prcopt_t *prcopt I  rtk processing options
*          solopt_t *solopt I  solution options
*                              solopt[0]=solution 1 options
*                              solopt[1]=solution 2 options
*          stream_t *moni   I  monitor stream (NULL: not used)
*          char   *errmsg   O  error message
* return : status (1:ok 0:error)
*-----------------------------------------------------------------------------*/
extern int rtksvrstart(rtksvr_t *svr, prcopt_t *prcopt)
{
    gtime_t time;
    int i,j,rw;
	eph_t eph;
    
    tracet(3,"rtksvrinit:\n");
    
    svr->cputime=svr->obsf=0;
    
    memset(&svr->nav,0,sizeof(nav_t));
    for (i=0;i<MAXEPH;i++) {
		if (readeph(i,&eph) > 0)
			svr->nav.eph[i]=eph;
    }
    svr->nav.n =MAXEPH;
#if ENAGLO
    svr->nav.ng=NSATGLO;
#endif
#if ENSBAS
    svr->nav.ns=NSATSBS;
#endif
    
    initlock(&svr->lock);

    rtkinit(&svr->rtk,prcopt);
    
    for (i=0;i<NSTAQ;i++) { /* input/log streams */
        /* initialize receiver rtcm control */
        init_rtcm(svr->rtcm+i);
    }
    /* set base station position */
    if (prcopt->refpos!=POSOPT_SINGLE) {
        for (i=0;i<6;i++) {
            svr->rtk.rb[i]=i<3?prcopt->rb[i]:0.0;
        }
    }
    
	rtksvrthread(svr);
    return 1;
}


